import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from face_detect_info.srv import FaceInfo
import cv2
from cv_bridge import CvBridge
import time
import threading


class FaceDetectClient(Node):
    def __init__(self, name):
        super().__init__(node_name=name)
        self._client = self.create_client(FaceInfo, "/face_dect")
        self._bridge = CvBridge()
        self._thd = threading.Thread(target=self.run)
        self._thd.start()

    def run(self):
        cap = cv2.VideoCapture(0)
        while True:
            ret, img = cap.read()
            if ret is None:
                break
            self._img = img
            self._send_request(img)
            time.sleep(1)
        cap.release()
        # cv2.destroyAllwindows()

    def _send_request(self, img):
        while self._client.wait_for_service(timeout_sec=1) is False:
            self.get_logger().info(
                f"wait for service {self._time_translate(time.time())}"
            )
            time.sleep(1)
        request = FaceInfo.Request()
        request.image = self._bridge.cv2_to_imgmsg(img)

        future = self._client.call_async(request)
        future.add_done_callback(self._request_callback)
        self.get_logger().info(f"send request, {self._time_translate(time.time())}")

    def _show_face_location(self, response, img):
        for i in range(response.number):
            top = response.top[i]
            right = response.right[i]
            bottom = response.bottom[i]
            left = response.left[i]
            cv2.rectangle(img, (left, top), (right, bottom), (0, 0, 255), 2)
            self.get_logger().info(f"image center:{(top + bottom)/2},{(left+right)/2}")

        cv2.imshow("face rec", img)
        cv2.waitKey(10)

    def _time_translate(self, time_in):
        time_struct = time.localtime(time_in)
        time_standard = time.strftime("%Y-%m-%d %H:%M:%S", time_struct)
        return time_standard

    def _request_callback(self, result_future):
        self.get_logger().info(f"recieve response, {self._time_translate(time.time())}")

        response = result_future.result()
        if response.invalid == False:
            self.get_logger().info(
                f"recieve response, people:{response.number}  \
                    del time:{response.use_time}  \
                    {self._time_translate(time.time())}"
            )
            # self._show_face_location(response, self._img)


def main():
    rclpy.init()
    node = FaceDetectClient("face_detect_client")
    rclpy.spin(node)
    rclpy.shutdown()
